
#include "qyAABB.h"
#include <utility>


QY_INLINE static void _swap( float &a, float &b )
{
    float temp = a;
    a = b;
    b = temp;
}

//-----------------------------------------------------------------------
bool AABB::LineIntersection(const Vector3 &start, const Vector3 &end) const
{
	float ld[3];
	Vector3 center = ( vecs[0] + vecs[1] ) * 0.5f;
	Vector3 extents = vecs[1] - center;
	Vector3 lineDir = 0.5f * ( end - start );
	Vector3 lineCenter = start + lineDir;
	Vector3 dir = lineCenter - center;

	ld[0] = fabs( lineDir[0] );
	if ( fabs( dir[0] ) > extents[0] + ld[0] ) {
		return false;
	}

	ld[1] = fabs( lineDir[1] );
	if ( fabs( dir[1] ) > extents[1] + ld[1] ) {
		return false;
	}

	ld[2] = fabs( lineDir[2] );
	if ( fabs( dir[2] ) > extents[2] + ld[2] ) {
		return false;
	}

	Vector3 cross = lineDir.Cross( dir );

	if ( fabs( cross[0] ) > extents[1] * ld[2] + extents[2] * ld[1] ) {
		return false;
	}

	if ( fabs( cross[1] ) > extents[0] * ld[2] + extents[2] * ld[0] ) {
		return false;
	}

	if ( fabs( cross[2] ) > extents[0] * ld[1] + extents[1] * ld[0] ) {
		return false;
	}

	return true;
}

//	//-----------------------------------------------------------------------

bool AABB::RayIntersection(const Vector3 &orig, const Vector3 &dir, float &scale_0 ) const
{
	float scale_1;
	const Vector3& min = vecs[0];
	const Vector3& max = vecs[1];
	const Vector3& rayorig = orig;
	const Vector3& raydir = dir;

	Vector3 absDir;
	absDir[0] = abs(raydir[0]);
	absDir[1] = abs(raydir[1]);
	absDir[2] = abs(raydir[2]);

	// Sort the axis, ensure check minimise floating error axis first
	int imax = 0, imid = 1, imin = 2;
	if (absDir[0] < absDir[2])
	{
		imax = 2;
		imin = 0;
	}
	if (absDir[1] < absDir[imin])
	{
		imid = imin;
		imin = 1;
	}
	else if (absDir[1] > absDir[imax])
	{
		imid = imax;
		imax = 1;
	}

	float start = 0, end = 100000000.0f;


#define _CALC_AXIS(i)                                       \
	do {                                                    \
	float denom = 1 / raydir[i];                         \
	float newstart = (min[i] - rayorig[i]) * denom;      \
	float newend = (max[i] - rayorig[i]) * denom;        \
	if (newstart > newend) _swap(newstart, newend); \
	if (newstart > end || newend < start) return false; \
	if (newstart > start) start = newstart;             \
	if (newend < end) end = newend;                     \
	} while(0)

	// Check each axis in turn

	_CALC_AXIS(imax);

	if (absDir[imid] < 0.00001f )
	{
		// Parallel with middle and minimise axis, check bounds only
		if (rayorig[imid] < min[imid] || rayorig[imid] > max[imid] ||
			rayorig[imin] < min[imin] || rayorig[imin] > max[imin])
			return false;
	}
	else
	{
		_CALC_AXIS(imid);

		if (absDir[imin] < 0.00001f )
		{
			// Parallel with minimise axis, check bounds only
			if (rayorig[imin] < min[imin] || rayorig[imin] > max[imin])
				return false;
		}
		else
		{
			_CALC_AXIS(imin);
		}
	}
#undef _CALC_AXIS
	scale_0 = start;
	scale_1 = end;

	return true;
}

//-----------------------------------------------------------------------
//bool AABB::RayIntersection(const SO::Vector3 &start, const SO::Vector3 &dir, float &scale) const
//{
//	int i, ax0, ax1, ax2, side, inside;
//	float f;
//	Vector3 hit;

//	ax0 = -1;
//	inside = 0;
//	for ( i = 0; i < 3; i++ ) {
//		if ( start[i] < vecs[0][i] ) {
//			side = 0;
//		}
//		else if ( start[i] > vecs[1][i] ) {
//			side = 1;
//		}
//		else {
//			inside++;
//			continue;
//		}
//		if ( dir[i] == 0.0f ) {
//			continue;
//		}
//		f = ( start[i] - vecs[side][i] );
//		if ( ax0 < 0 || fabs( f ) > fabs( scale * dir[i] ) ) {
//			scale = - ( f / dir[i] );
//			ax0 = i;
//		}
//	}

//	if ( ax0 < 0 ) {
//		scale = 0.0f;
//		// return true if the start point is inside the bounds
//		return ( inside == 3 );
//	}

//	ax1 = (ax0+1)%3;
//	ax2 = (ax0+2)%3;
//	hit[ax1] = start[ax1] + scale * dir[ax1];
//	hit[ax2] = start[ax2] + scale * dir[ax2];

//	return ( hit[ax1] >= vecs[0][ax1] && hit[ax1] <= vecs[1][ax1] &&
//		hit[ax2] >= vecs[0][ax2] && hit[ax2] <= vecs[1][ax2] );
//}

